/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_FlightProcedure.c
 * FP means FlightProcedure
 *  Created on: 2022年12月30日
 *      Author: 64435
 */

#include "LOS_FATFS.h"
#include "LOS_FlightProcedure.h"


extern FIL rfile_task;
FLIGHTTASK_t* phead;

//CRC32表
uint32_t crc_tab[] ={0x0,0xf26b8303,0xe13b70f7,0x1350f3f4,0xc79a971f,0x35f1141c,0x26a1e7e8,0xd4ca64eb,0x8ad958cf,0x78b2dbcc,0x6be22838,0x9989ab3b,0x4d43cfd0,0xbf284cd3,0xac78bf27,0x5e133c24,0x105ec76f,0xe235446c,
0xf165b798,0x30e349b,0xd7c45070,0x25afd373,0x36ff2087,0xc494a384,0x9a879fa0,0x68ec1ca3,0x7bbcef57,0x89d76c54,0x5d1d08bf,0xaf768bbc,0xbc267848,0x4e4dfb4b,0x20bd8ede,0xd2d60ddd,0xc186fe29,0x33ed7d2a,
0xe72719c1,0x154c9ac2,0x61c6936,0xf477ea35,0xaa64d611,0x580f5512,0x4b5fa6e6,0xb93425e5,0x6dfe410e,0x9f95c20d,0x8cc531f9,0x7eaeb2fa,0x30e349b1,0xc288cab2,0xd1d83946,0x23b3ba45,0xf779deae,0x5125dad,
0x1642ae59,0xe4292d5a,0xba3a117e,0x4851927d,0x5b016189,0xa96ae28a,0x7da08661,0x8fcb0562,0x9c9bf696,0x6ef07595,0x417b1dbc,0xb3109ebf,0xa0406d4b,0x522bee48,0x86e18aa3,0x748a09a0,0x67dafa54,0x95b17957,
0xcba24573,0x39c9c670,0x2a993584,0xd8f2b687,0xc38d26c,0xfe53516f,0xed03a29b,0x1f682198,0x5125dad3,0xa34e59d0,0xb01eaa24,0x42752927,0x96bf4dcc,0x64d4cecf,0x77843d3b,0x85efbe38,0xdbfc821c,0x2997011f,
0x3ac7f2eb,0xc8ac71e8,0x1c661503,0xee0d9600,0xfd5d65f4,0xf36e6f7,0x61c69362,0x93ad1061,0x80fde395,0x72966096,0xa65c047d,0x5437877e,0x4767748a,0xb50cf789,0xeb1fcbad,0x197448ae,0xa24bb5a,0xf84f3859,
0x2c855cb2,0xdeeedfb1,0xcdbe2c45,0x3fd5af46,0x7198540d,0x83f3d70e,0x90a324fa,0x62c8a7f9,0xb602c312,0x44694011,0x5739b3e5,0xa55230e6,0xfb410cc2,0x92a8fc1,0x1a7a7c35,0xe811ff36,0x3cdb9bdd,0xceb018de,
0xdde0eb2a,0x2f8b6829,0x82f63b78,0x709db87b,0x63cd4b8f,0x91a6c88c,0x456cac67,0xb7072f64,0xa457dc90,0x563c5f93,0x82f63b7,0xfa44e0b4,0xe9141340,0x1b7f9043,0xcfb5f4a8,0x3dde77ab,0x2e8e845f,0xdce5075c,
0x92a8fc17,0x60c37f14,0x73938ce0,0x81f80fe3,0x55326b08,0xa759e80b,0xb4091bff,0x466298fc,0x1871a4d8,0xea1a27db,0xf94ad42f,0xb21572c,0xdfeb33c7,0x2d80b0c4,0x3ed04330,0xccbbc033,0xa24bb5a6,0x502036a5,
0x4370c551,0xb11b4652,0x65d122b9,0x97baa1ba,0x84ea524e,0x7681d14d,0x2892ed69,0xdaf96e6a,0xc9a99d9e,0x3bc21e9d,0xef087a76,0x1d63f975,0xe330a81,0xfc588982,0xb21572c9,0x407ef1ca,0x532e023e,0xa145813d,
0x758fe5d6,0x87e466d5,0x94b49521,0x66df1622,0x38cc2a06,0xcaa7a905,0xd9f75af1,0x2b9cd9f2,0xff56bd19,0xd3d3e1a,0x1e6dcdee,0xec064eed,0xc38d26c4,0x31e6a5c7,0x22b65633,0xd0ddd530,0x417b1db,0xf67c32d8,
0xe52cc12c,0x1747422f,0x49547e0b,0xbb3ffd08,0xa86f0efc,0x5a048dff,0x8ecee914,0x7ca56a17,0x6ff599e3,0x9d9e1ae0,0xd3d3e1ab,0x21b862a8,0x32e8915c,0xc083125f,0x144976b4,0xe622f5b7,0xf5720643,0x7198540,
0x590ab964,0xab613a67,0xb831c993,0x4a5a4a90,0x9e902e7b,0x6cfbad78,0x7fab5e8c,0x8dc0dd8f,0xe330a81a,0x115b2b19,0x20bd8ed,0xf0605bee,0x24aa3f05,0xd6c1bc06,0xc5914ff2,0x37faccf1,0x69e9f0d5,0x9b8273d6,
0x88d28022,0x7ab90321,0xae7367ca,0x5c18e4c9,0x4f48173d,0xbd23943e,0xf36e6f75,0x105ec76,0x12551f82,0xe03e9c81,0x34f4f86a,0xc69f7b69,0xd5cf889d,0x27a40b9e,0x79b737ba,0x8bdcb4b9,0x988c474d,0x6ae7c44e,
0xbe2da0a5,0x4c4623a6,0x5f16d052,0xad7d5351};
//CRC8表
uint8_t CrcTable[] = {
 0x00, 0x07, 0x0E, 0x09, 0x1C, 0x1B, 0x12, 0x15,
 0x38, 0x3F, 0x36, 0x31, 0x24, 0x23, 0x2A, 0x2D,
 0x70, 0x77, 0x7E, 0x79, 0x6C, 0x6B, 0x62, 0x65,
 0x48, 0x4F, 0x46, 0x41, 0x54, 0x53, 0x5A, 0x5D,
 0xE0, 0xE7, 0xEE, 0xE9, 0xFC, 0xFB, 0xF2, 0xF5,
 0xD8, 0xDF, 0xD6, 0xD1, 0xC4, 0xC3, 0xCA, 0xCD,
 0x90, 0x97, 0x9E, 0x99, 0x8C, 0x8B, 0x82, 0x85,
 0xA8, 0xAF, 0xA6, 0xA1, 0xB4, 0xB3, 0xBA, 0xBD,
 0xC7, 0xC0, 0xC9, 0xCE, 0xDB, 0xDC, 0xD5, 0xD2,
 0xFF, 0xF8, 0xF1, 0xF6, 0xE3, 0xE4, 0xED, 0xEA,
 0xB7, 0xB0, 0xB9, 0xBE, 0xAB, 0xAC, 0xA5, 0xA2,
 0x8F, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9D, 0x9A,
 0x27, 0x20, 0x29, 0x2E, 0x3B, 0x3C, 0x35, 0x32,
 0x1F, 0x18, 0x11, 0x16, 0x03, 0x04, 0x0D, 0x0A,
 0x57, 0x50, 0x59, 0x5E, 0x4B, 0x4C, 0x45, 0x42,
 0x6F, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7D, 0x7A,
 0x89, 0x8E, 0x87, 0x80, 0x95, 0x92, 0x9B, 0x9C,
 0xB1, 0xB6, 0xBF, 0xB8, 0xAD, 0xAA, 0xA3, 0xA4,
 0xF9, 0xFE, 0xF7, 0xF0, 0xE5, 0xE2, 0xEB, 0xEC,
 0xC1, 0xC6, 0xCF, 0xC8, 0xDD, 0xDA, 0xD3, 0xD4,
 0x69, 0x6E, 0x67, 0x60, 0x75, 0x72, 0x7B, 0x7C,
 0x51, 0x56, 0x5F, 0x58, 0x4D, 0x4A, 0x43, 0x44,
 0x19, 0x1E, 0x17, 0x10, 0x05, 0x02, 0x0B, 0x0C,
 0x21, 0x26, 0x2F, 0x28, 0x3D, 0x3A, 0x33, 0x34,
 0x4E, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5C, 0x5B,
 0x76, 0x71, 0x78, 0x7F, 0x6A, 0x6D, 0x64, 0x63,
 0x3E, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2C, 0x2B,
 0x06, 0x01, 0x08, 0x0F, 0x1A, 0x1D, 0x14, 0x13,
 0xAE, 0xA9, 0xA0, 0xA7, 0xB2, 0xB5, 0xBC, 0xBB,
 0x96, 0x91, 0x98, 0x9F, 0x8A, 0x8D, 0x84, 0x83,
 0xDE, 0xD9, 0xD0, 0xD7, 0xC2, 0xC5, 0xCC, 0xCB,
 0xE6, 0xE1, 0xE8, 0xEF, 0xFA, 0xFD, 0xF4, 0xF3};
 //CRC16表
 static const uint16_t CRC16_TABLE[256]=
{
    0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
    0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,
    0x1231,0x0210,0x3273,0x2252,0x52b5,0x4294,0x72f7,0x62d6,
    0x9339,0x8318,0xb37b,0xa35a,0xd3bd,0xc39c,0xf3ff,0xe3de,
    0x2462,0x3443,0x0420,0x1401,0x64e6,0x74c7,0x44a4,0x5485,
    0xa56a,0xb54b,0x8528,0x9509,0xe5ee,0xf5cf,0xc5ac,0xd58d,
    0x3653,0x2672,0x1611,0x0630,0x76d7,0x66f6,0x5695,0x46b4,
    0xb75b,0xa77a,0x9719,0x8738,0xf7df,0xe7fe,0xd79d,0xc7bc,
    0x48c4,0x58e5,0x6886,0x78a7,0x0840,0x1861,0x2802,0x3823,
    0xc9cc,0xd9ed,0xe98e,0xf9af,0x8948,0x9969,0xa90a,0xb92b,
    0x5af5,0x4ad4,0x7ab7,0x6a96,0x1a71,0x0a50,0x3a33,0x2a12,
    0xdbfd,0xcbdc,0xfbbf,0xeb9e,0x9b79,0x8b58,0xbb3b,0xab1a,
    0x6ca6,0x7c87,0x4ce4,0x5cc5,0x2c22,0x3c03,0x0c60,0x1c41,
    0xedae,0xfd8f,0xcdec,0xddcd,0xad2a,0xbd0b,0x8d68,0x9d49,
    0x7e97,0x6eb6,0x5ed5,0x4ef4,0x3e13,0x2e32,0x1e51,0x0e70,
    0xff9f,0xefbe,0xdfdd,0xcffc,0xbf1b,0xaf3a,0x9f59,0x8f78,
    0x9188,0x81a9,0xb1ca,0xa1eb,0xd10c,0xc12d,0xf14e,0xe16f,
    0x1080,0x00a1,0x30c2,0x20e3,0x5004,0x4025,0x7046,0x6067,
    0x83b9,0x9398,0xa3fb,0xb3da,0xc33d,0xd31c,0xe37f,0xf35e,
    0x02b1,0x1290,0x22f3,0x32d2,0x4235,0x5214,0x6277,0x7256,
    0xb5ea,0xa5cb,0x95a8,0x8589,0xf56e,0xe54f,0xd52c,0xc50d,
    0x34e2,0x24c3,0x14a0,0x0481,0x7466,0x6447,0x5424,0x4405,
    0xa7db,0xb7fa,0x8799,0x97b8,0xe75f,0xf77e,0xc71d,0xd73c,
    0x26d3,0x36f2,0x0691,0x16b0,0x6657,0x7676,0x4615,0x5634,
    0xd94c,0xc96d,0xf90e,0xe92f,0x99c8,0x89e9,0xb98a,0xa9ab,
    0x5844,0x4865,0x7806,0x6827,0x18c0,0x08e1,0x3882,0x28a3,
    0xcb7d,0xdb5c,0xeb3f,0xfb1e,0x8bf9,0x9bd8,0xabbb,0xbb9a,
    0x4a75,0x5a54,0x6a37,0x7a16,0x0af1,0x1ad0,0x2ab3,0x3a92,
    0xfd2e,0xed0f,0xdd6c,0xcd4d,0xbdaa,0xad8b,0x9de8,0x8dc9,
    0x7c26,0x6c07,0x5c64,0x4c45,0x3ca2,0x2c83,0x1ce0,0x0cc1,
    0xef1f,0xff3e,0xcf5d,0xdf7c,0xaf9b,0xbfba,0x8fd9,0x9ff8,
    0x6e17,0x7e36,0x4e55,0x5e74,0x2e93,0x3eb2,0x0ed1,0x1ef0
};

uint8_t LOS_FP_Procedure(uint32_t sec)
{
    switch(sec)
    {
        //UHF1天线热刀上电
        case 10:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKUHF1_CODE] = 0XAA;
        }
        break;

        //VHF1天线热刀上电
        case 25:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKVHF1_CODE] = 0XAA;
        }
        break;

        //UHF2天线热刀上电
        case 40:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKUHF2_CODE] = 0XAA;
        }
        break;

        //VHF2天线热刀上电
        case 55:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKVHF2_CODE] = 0XAA;
        }
        break;

        //遮光罩热刀上电
        case 70:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKLEN1_CODE] = 0XAA;
            LOS_TK_SEL[TKLEN2_CODE] = 0XAA;
        }
        break;

        case 85:
        LOS_YH50_PWROn();
        break;

        case 90:
        LOS_MagMe_PWROn();
        break;

        case 100:
        LOS_CAN_Radio();
        return 1; //进入姿控程序

        default:

    }

    if (sec > 100)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

void LOS_FP_ExtraTK(uint32_t sec)
{
    switch(sec)
    {
        //UHF1天线热刀上电
        case 10:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKUHF1_CODE] = 0XAA;
        }
        break;

        //VHF1天线热刀上电
        case 25:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKVHF1_CODE] = 0XAA;
        }
        break;

        //UHF2天线热刀上电
        case 40:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKUHF2_CODE] = 0XAA;
        }
        break;

        //VHF2天线热刀上电
        case 55:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKVHF2_CODE] = 0XAA;
        }
        break;

        //遮光罩热刀上电
        case 70:
        if (FR_TKANTEN == 0XABED5CFA)
        {
            LOS_TK_SEL[TKLEN1_CODE] = 0XAA;
            LOS_TK_SEL[TKLEN2_CODE] = 0XAA;
        }
        break;
    }
}


FLIGHTTASK_t* LOS_FP_InitTable()
{
    FLIGHTTASK_t* guard = (FLIGHTTASK_t*)pvPortMalloc(sizeof(FLIGHTTASK_t));   //创建头节点
    if (guard == NULL)
    {
        return -1;
    }

    guard -> prev = guard;
    guard -> next = guard;
    return guard;
}

FLIGHTTASK_t* LOS_FP_ApplyNode(uint32_t num, uint32_t time)
{
    FLIGHTTASK_t* node = (FLIGHTTASK_t*)pvPortMalloc(sizeof(FLIGHTTASK_t));

    if (node == NULL)
    {
        return -1;
    }

    node -> extime = time;
    node -> tasknum = num;
    node -> prev = NULL;
    node -> next = NULL;
    return node;
}

FLIGHTTASK_t* LOS_FP_PushBack(uint32_t num, uint32_t time)  //尾插
{
    FLIGHTTASK_t* tail = phead->prev;
    FLIGHTTASK_t* newnode = LOS_FP_ApplyNode(num, time);

    tail -> next = newnode;
    newnode -> prev = tail;
    newnode -> next = phead;
    phead -> prev = newnode;
    return newnode;
}

void LOS_FP_PushFront(uint32_t num, uint32_t time)
{
    FLIGHTTASK_t* first = phead->next;
    FLIGHTTASK_t* newnode = LOS_FP_ApplyNode(num, time);

    phead -> next = newnode;
    newnode -> prev = phead;
    newnode -> next = first;
    first -> prev = newnode;
}


FLIGHTTASK_t* LOS_FP_FindTimeUp(uint32_t time)
{
    if (phead == NULL)
    {
        phead = LOS_FP_InitTable();
    }
    FLIGHTTASK_t* current = phead -> next;
    while(current != phead)
    {
        if ((current -> extime == time)||(current -> extime == 0))
        {
            return current;
        }
        current = current -> next;
    }

    return current;
}


void LOS_FP_DeleteNode(FLIGHTTASK_t* pos)
{
    FLIGHTTASK_t* later = pos -> next;
    FLIGHTTASK_t* last = pos -> prev;
    last -> next = later;
    later -> prev = last;
    vPortFree(pos->arg); //每个链表项的参数内存是动态分配的，记得释放
    pos -> arg = NULL;
    vPortFree(pos);
    pos = NULL;
}


uint32_t LOS_FP_GetTableSize()  //获取当前任务数
{
    FLIGHTTASK_t* current = phead -> next;
    uint32_t size = 0;
    while(current!=phead)
    {
        size++;
        current = current -> next;
    }
    return size;
}


FLIGHTTASK_t* LOS_FP_CreateTable(uint32_t tasknum, uint32_t extime)  //创建任务链表
{
    //static uint8_t enterflag = 0;//0:第一次运行该程序；其他值：已经运行过至少一遍。用于判断是否需要初始化链表
    if (phead == NULL)
    {
        phead = LOS_FP_InitTable();
        //enterflag++;
    }
    return LOS_FP_PushBack(tasknum,extime);
}


void LOS_FP_CheckTable(uint32_t time)
{
    FLIGHTTASK_t* extask;
    do
    {
        extask = LOS_FP_FindTimeUp(time);
        if (extask != phead)
        {
            LOS_FP_TCEX(extask);
            LOS_FP_DeleteNode(extask);
        }
    }while(extask != phead);  //持续检测是否有需要执行的任务
    
}

FLIGHTTASK_t* LOS_FP_SearchTask(uint32_t num)
{
    FLIGHTTASK_t* current = phead -> next;
    while(current != phead)
    {
        if (current -> tasknum == num )
        {
            return current;
        }
        current = current -> next;
    }

    return NULL;
}


void LOS_FP_FindFirstDelayCMD()
{
    uint32_t last_time = 0;
    uint32_t last_cmd = 0;
    if (phead == NULL)
    {
        phead = LOS_FP_InitTable();
    }
    FLIGHTTASK_t* current = phead -> next;
    while(current != phead)
    {
        if (current -> extime != 0)
        {
            if (last_time == 0)
            {
                last_time = current -> extime;
                last_cmd = current -> tasknum;
            }
            else if ((current -> extime) < last_time)
            {
                last_time = current -> extime;
                last_cmd = current -> tasknum;
            }
            current = current -> next;
        }
        else
        {
            current = current -> next;
        }
    }

    LOS_TC_FirstDelayCMD = last_cmd;
    LOS_TC_FirstDelayTime = last_time;
}


void LOS_FP_DBGPRT()
{
    FLIGHTTASK_t* current = phead -> next;
    uint16_t cnt = 0;
    uint8_t pack[300];
    while(current!=phead)
    {
        memcpy(&pack[cnt], &current->tasknum, 4);
        memcpy(&pack[cnt+4], &current->extime, 4);
        cnt+=8;
        current = current -> next;
    }
    LOS_CAN_SendPack(LOS_CAN_NUM2,0xFC,pack,cnt);
}

void LOS_FP_SafeMode()
{
    LOS_SAT_SoftSafeMode = 1;
    LOS_SAT_SafeModeCNT++;
}

void LOS_FP_ExitSafeMode()
{
    LOS_SAT_SoftSafeMode = 0;
}

/** @fn uint32_t LOS_FP_CRC32Cal(const uint8_t * data, uint32_t length, uint32_t crc)
*   @brief CRC32值计算
*   @param[in] data 要计算的数据包
*   @param[in] length 数据包长度
*   @param[in] crc CRC32初始值,第一包取0x00000000，后续包取上一包的计算结果
*   @return crc32计算值
*   @note 该函数进行CRC32校验值计算，使用查表法，多项式为0x82F63B78L。
*/
uint32_t LOS_FP_CRC32Cal(const uint8_t* data, uint32_t length, uint32_t crc)
{
    while (length--)
	   crc = crc_tab[(crc ^ *data++) & 0xFFL] ^ (crc >> 8);

   return crc ^ 0xFFFFFFFF;  //最后这个异或别忘了，要跟电脑上的处理统一起来
}

/** @fn uint8_t LOS_FP_CRC8Cal(uint8_t* buf, uint32_t length)
*   @brief CRC8值计算
*   @param[in] buf 要计算的数据包
*   @param[in] length 数据包长度
*   @return crc8计算值
*   @note 该函数进行CRC8校验值计算，使用查表法
*/
uint8_t LOS_FP_CRC8Cal(uint8_t* buf, uint32_t length)
{
    uint8_t crc = 0;
    uint32_t temp = 0;
    uint32_t cnt = 0;
    for (cnt = 0; cnt < length; cnt++)
    {
        temp = crc ^ buf[cnt];
        crc = CrcTable[temp];
    }

    return crc;
}

/** @fn uint16_t LOS_FP_CRC16Cal(uint8_t* data_buf, uint8_t start_byte, uint8_t length)
*   @brief CRC16值计算
*   @param[in] data_buf 要计算的数据包
*   @param[in] start_byte 起始字节索引
*   @param[in] length 数据长度
*   @return crc16计算值
*   @note 该函数进行CRC16校验值计算，使用查表法，由阿穆尔国立大学(AMSU)提供
*/
uint16_t LOS_FP_CRC16Cal(uint8_t* data_buf, uint8_t start_byte, uint8_t length)
{
    uint8_t i;
    uint16_t retval;

    retval = 0;
    for (i = start_byte; i < start_byte + length; i++)
    {
        retval = ((retval<<8)  & 0xFFFF) ^ CRC16_TABLE[((retval>>8) ^ *(data_buf + i)) & 0x00FF];
    }
    return retval;
}

/** @fn size_t LOS_FP_CobsEncode(const void *data, size_t length, uint8_t *buffer)
*   @brief COBS编码函数，用于阿穆尔光电传感器
	@param data Pointer to input data to encode
	@param length Number of bytes to encode
	@param buffer Pointer to encoded output buffer
	@return Encoded buffer length in bytes
	@note Does not output delimiter byte
*/
size_t LOS_FP_CobsEncode(const void *data, size_t length, uint8_t *buffer)
{
	uint8_t *encode = buffer; // Encoded byte pointer
	uint8_t *codep = encode++; // Output code pointer
	uint8_t code = 1; // Code value
    const uint8_t *byte;

	for (byte = (const uint8_t *)data; length--; ++byte)
	{
		if (*byte) // Byte not zero, write it
			*encode++ = *byte, ++code;

		if (!*byte || code == 0xff) // Input is zero or block completed, restart
		{
			*codep = code, code = 1, codep = encode;
			if (!*byte || length)
				++encode;
		}
	}
	*codep = code; // Write final code value

	return (size_t)(encode - buffer);
}

/** @fn size_t cobsDecode(const uint8_t *buffer, size_t length, void *data)
    @brief COBS解码函数，用于阿穆尔光电传感器
	@param buffer Pointer to encoded input bytes
	@param length Number of bytes to decode
	@param data Pointer to decoded output data
	@return Number of bytes successfully decoded
	@note Stops decoding if delimiter byte is found
*/
size_t LOS_FP_CobsDecode(const uint8_t *buffer, size_t length, void *data)
{
	const uint8_t *byte = buffer; // Encoded input byte pointer
	uint8_t *decode = (uint8_t *)data; // Decoded output byte pointer
    uint8_t code = 0xff;
    uint8_t block = 0;

	for (code = 0xff, block = 0; byte < buffer + length; --block)
	{
		if (block) // Decode block byte
			*decode++ = *byte++;
		else
		{
			block = *byte++;             // Fetch the next block length
			if (block && (code != 0xff)) // Encoded zero, write it unless it's delimiter.
				*decode++ = 0;
			code = block;
			if (!code) // Delimiter code found
				break;
		}
	}

	return (size_t)(decode - (uint8_t *)data);
}

/** @fn void LOS_FP_BLEndianTransfer(uint8_t* input, uint8_t length)
*   @brief 大小端转换（不大于32个字节）
*   @param[in] input 要处理的字节串 
*   @param[in] length 字节串的长度
*   @note 该函数把输入的input字节串调整成倒序
*/
void LOS_FP_BLEndianTransfer(uint8_t* input, uint8_t length)
{
    static uint8_t buf[32];  //最多转32个字节
    uint8_t cnt = 0;
    if (length < 32)
    {
        for (cnt = 0; cnt < length; cnt++)
        {
            buf[length - cnt - 1] = input[cnt];
        }
        memcpy(input, buf, length);
    }
}

/** @fn void LOS_FP_EnterNewModeSwitch(uint16_t mode)
*   @brief 进入新模式加断电序列
*   @param[in] mode 要进入的模式 
*   @note 该函数根据输入的模式，决定哪些单机需要加断电
*/
void LOS_FP_EnterNewModeSwitch(uint16_t mode)
{
    switch(mode)
    {

//        case Command:
//        {
//            LOS_GNSS_PWROn();
//            LOS_FW_PWROn(FW_X);
//            LOS_FW_PWROn(FW_Y);
//            LOS_FW_PWROn(FW_Z);
//            LOS_SUNS_PWROn();
//            LOS_YH50_PWROn();
//            LOS_MagMe_PWROn();
//            LOS_SSY_PWROn();
//            LOS_SSZ_PWROn();
//        }
//        break;
//
//        case Safe:  //安全模式有下电序列
//        {
//            LOS_EPS_TurnOffAll();
//        }
//        break;
//
//        case SunPoint:
//        {
//            LOS_FW_PWROn(FW_X);
//            LOS_FW_PWROn(FW_Y);
//            LOS_FW_PWROn(FW_Z);
//            LOS_SUNS_PWROn();
//            LOS_YH50_PWROn();
//            LOS_MagMe_PWROn();
//            LOS_SSY_PWROn();
//            LOS_SSZ_PWROn();
//        }
//        break;
//
//        case SunSearch:  //单轴对日有下电序列
//        {
//            LOS_EPS_TurnOffAll();
//            LOS_SUNS_PWROn();
//            LOS_FW_PWROn(FW_X);
//            LOS_FW_PWROn(FW_Y);
//            LOS_FW_PWROn(FW_Z);
//            LOS_MagMe_PWROn();
//            LOS_YH50_PWROn();
//        }
//        break;
//
//        case Damping:
//        {
//            LOS_EPS_TurnOffAll();
//            LOS_YH50_PWROn();
//            LOS_MagMe_PWROn();
//        }
//        break;
//
//        case CamTarget:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case CamGeo:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case CMOSTarget:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case CMOSGeo:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case XHelicalTarget:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case XHelicalGeo:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case XMicroTarget:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;
//
//        case XMicroGeo:
//        {
//           LOS_GNSS_PWROn();
//           LOS_FW_PWROn(FW_X);
//           LOS_FW_PWROn(FW_Y);
//           LOS_FW_PWROn(FW_Z);
//           LOS_SUNS_PWROn();
//           LOS_YH50_PWROn();
//           LOS_MagMe_PWROn();
//           LOS_SSY_PWROn();
//           LOS_SSZ_PWROn();
//        }
//        break;

        default:

    }
}

